#!/usr/bin/env python
# -*- coding: utf-8 -*-

"""
ROS 2 节点, 实现了机器狗和机械臂的统一手柄控制。

本节点通过一个 Xbox 手柄, 实现了在 "机器狗控制模式" 和 "机械臂控制模式" 之间的
动态切换。
- 机器狗模式: 控制机器狗的站立、趴下、平移和旋转。
- 机械臂模式: 控制机械臂的关节运动、笛卡尔空间运动以及夹爪的开合。

模式切换:
- 启动时为 [停止] 模式。
- [停止] -> [机器狗]: 按 'A' 键 (机器狗站立)。
- [机器狗] -> [停止]: 按 'Y' 键 (机器狗趴下)。
- [机器狗] <-> [机械臂]: 长按 'RB' 键。
- [停止] -> [机械臂]: 长按 'RB' 键。
- 机械臂内部模式 [关节] <-> [笛卡尔]: 长按 'LB' 键。
"""

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.duration import Duration

from sensor_msgs.msg import Joy, JointState
from std_msgs.msg import Float64MultiArray
from ros_alphadog.msg import SetVelocity, SetRpy, RobotReady, DogCtrlStateStamped
from agent_msgs.action import Execute

from pymoveit2.moveit2_servo import MoveIt2Servo

import math
import json
import pathlib

# ===============================================================
# 1. 统一的配置和常量
# ===============================================================

# 控制模式定义
MODE_STOP = 0
MODE_DOG_CONTROL = 1
MODE_ARM_CONTROL = 2

# Xbox 手柄按键和摇杆索引 (通用)
BUTTON_A_INDEX = 0      # 机器狗站立 / 机械臂启用
BUTTON_B_INDEX = 1      # (未使用) / 机械臂禁用
BUTTON_X_INDEX = 2      # 机械臂一键归位 (Home)
BUTTON_Y_INDEX = 3      # 机器狗趴下 / 机械臂预设位置2
BUTTON_LB_INDEX = 4     # 机械臂关节/笛卡尔模式切换
BUTTON_RB_INDEX = 5     # 长按切换 机器狗/机械臂 模式
BUTTON_VIEW_INDEX = 6   # (未使用)
BUTTON_MENU_INDEX = 7   # (未使用)

AXIS_LEFT_STICK_X = 0   # 狗: 左右平移 / 臂: X轴移动
AXIS_LEFT_STICK_Y = 1   # 狗: 前后移动 / 臂: Y轴移动
AXIS_RIGHT_STICK_X = 3  # 狗: 旋转 / 臂: Z轴旋转(Yaw)
AXIS_RIGHT_STICK_Y = 4  # 狗: 俯仰 / 臂: Z轴移动
AXIS_DPAD_X = 6         # 臂: Y轴旋转(Pitch)
AXIS_DPAD_Y = 7         # (未使用)
AXIS_LT = 2             # 臂: 夹爪闭合
AXIS_RT = 5             # 臂: 夹爪张开

# 机器狗相关常量
DOG_FORWARD_SPEED_SCALE = 1.34
DOG_BACKWARD_SPEED_SCALE = 0.67
DOG_STRAFE_SPEED_SCALE = 0.67
DOG_ANGULAR_SPEED_SCALE = 6.3
DOG_PITCH_SCALE = 1.05
DOG_YAW_ANGLE_SCALE = 1.05
DOG_ESTOP_ACTION_ID = 0
DOG_STAND_UP_ACTION_ID = 4
DOG_GET_DOWN_ACTION_ID = 2

# 机械臂相关常量
ARM_JOINT_NAMES = ["1", "2", "3", "4", "5"]
ARM_JOINT_AXIS_MAP = [AXIS_LEFT_STICK_X, AXIS_LEFT_STICK_Y, AXIS_RIGHT_STICK_X, AXIS_RIGHT_STICK_Y, AXIS_DPAD_X]
ARM_JOINT_VELOCITY_GAIN = 1.5
GRIPPER_JOINT_NAME = "6"
GRIPPER_OPEN_POS = 1.745
GRIPPER_CLOSED_POS = -1.1745
GRIPPER_JOG_SPEED = 8.0

# 从 motor_bridge 复制的校准和电机定义
# (为了能正确解析校准文件并计算弧度限制)
CALIB_PATH = pathlib.Path.home() / ".calibrate" / "so101_follower_calibrate.json"
JOINTS = {
    # 只关心机械臂的关节
    "1": {"mode": "RANGE_M100_100"},
    "2": {"mode": "RANGE_M100_100"},
    "3": {"mode": "RANGE_M100_100"},
    "4": {"mode": "RANGE_M100_100"},
    "5": {"mode": "RANGE_M100_100"},
    "6": {"mode": "RANGE_0_100"},
}
class MotorNormMode: # 模拟 motor_bridge 中的 Enum
    RANGE_M100_100 = "RANGE_M100_100"
    RANGE_0_100 = "RANGE_0_100"

# 复位位置, 也从 motor_bridge 复制
RESET_NORMALIZED_GOALS = {
    "1": -17.00729927007299,
    "2": -70.61728395061728,
    "3": 97.65625,
    "4": -63.205980066445186,
    "5": -52.67399267399268,
    "6": 37.80091086532206, # 夹爪的复位位置, 此处仅为保持数据完整
}

# Y 键预设位置 (相对弧度)
PRESET_Y_ARM_RAD_GOALS = [
    -0.007669903939428124,
    0.053689327575997536,
    -3.118582941771508,
    0.0,
    -0.0015339807878855805
]


class UnifiedJoystickController(Node):
    def __init__(self):
        super().__init__('unified_joystick_controller')

        # ===============================================================
        # 2. 状态和变量初始化
        # ===============================================================
        self.control_mode = MODE_STOP
        self.latest_joy_data = None
        self.previous_buttons = None

        # 机器狗状态
        self.is_dog_physically_ready = False
        self.dog_ctrl_state = None

        # 机械臂状态
        self.is_arm_control_enabled = False # 机械臂的总使能开关 (A/B键)
        self.is_arm_joint_mode = False      # 机械臂的内部模式 (关节/笛卡尔)
        self.current_arm_positions = [0.0] * len(ARM_JOINT_NAMES)
        self.arm_home_position = [0.0] * len(ARM_JOINT_NAMES) # 'X' 键归位，机械臂的 "0" 位置 (相对)
        self.arm_preset_y_position = PRESET_Y_ARM_RAD_GOALS
        self.last_commanded_positions = [0.0] * len(ARM_JOINT_NAMES)
        self.first_joint_state_received = False
        self.arm_joint_indices = []
        self.current_gripper_pos = GRIPPER_OPEN_POS
        self.gripper_joint_idx = -1
        
        # 机械臂关节的相对弧度限制
        self.arm_relative_rad_limits = {} # 格式: { '1': (min_rel_rad, max_rel_rad), ... }

        # 模式切换逻辑变量
        self.long_press_duration = Duration(seconds=0.5)
        self.rb_press_time = None # 用于 狗/臂 模式切换
        self.lb_press_time = None # 用于 臂内部 模式切换

        # 用于打印模式状态的辅助字典
        self.axis_names = {
            0: "左摇杆 L/R", 1: "左摇杆 U/D", 2: "左扳机 (LT)",
            3: "右摇杆 L/R", 4: "右摇杆 U/D", 5: "右扳机 (RT)",
            6: "十字键 L/R", 7: "十字键 U/D"
        }
        self.arm_cartesian_axis_map = {
            "linear_x": AXIS_LEFT_STICK_X, "linear_y": AXIS_LEFT_STICK_Y, "linear_z": AXIS_RIGHT_STICK_Y,
            "angular_y": AXIS_DPAD_X, "angular_z": AXIS_RIGHT_STICK_X,
            "gripper_open": AXIS_RT, "gripper_close": AXIS_LT
        }

        # ===============================================================
        # 3. 初始化所有发布者、订阅者和Action客户端
        # ===============================================================
        # 机器狗
        self.dog_velocity_pub = self.create_publisher(SetVelocity, '/alphadog_node/set_velocity', 10)
        self.dog_rpy_pub = self.create_publisher(SetRpy, '/alphadog_node/set_rpy', 10)
        self.dog_action_client = ActionClient(self, Execute, '/agent_skill/do_action/execute')
        self.dog_ready_sub = self.create_subscription(RobotReady, '/alphadog_node/robot_ready', self.dog_ready_callback, 10)
        self.dog_state_sub = self.create_subscription(DogCtrlStateStamped, '/alphadog_node/dog_ctrl_state', self.dog_state_callback, 10)

        # 机械臂
        self.arm_servo = MoveIt2Servo(self, frame_id="base", linear_speed=1.0, angular_speed=1.0, enable_at_init=False)
        self.arm_direct_pos_pub = self.create_publisher(Float64MultiArray, "/arm_controller/commands", 10)
        self.gripper_pub = self.create_publisher(Float64MultiArray, "/gripper_controller/commands", 10)
        
        # 通用
        self.joy_sub = self.create_subscription(Joy, '/joy', self.joy_callback, 10)
        self.joint_state_sub = self.create_subscription(JointState, "/joint_states", self.joint_state_callback, 10)

        # 主控制循环定时器
        timer_period = 1.0 / 50.0  # 50Hz
        self.control_loop_timer = self.create_timer(timer_period, self.control_loop_callback)
        
        # 加载关节限制
        self._load_joint_limits()
        
        self.get_logger().info("✅ 统一手柄控制器已启动 (50Hz)。")
        self.get_logger().info("当前模式: [停止]. 按 'A' 键让机器狗站立。")
        self.get_logger().info("长按 'RB' 键 (>0.5s) 可在 [机器狗控制] 和 [机械臂控制] 之间切换。")
        self.get_logger().info(f"  - Y 键预设 (手臂) 已加载: { [round(p, 2) for p in self.arm_preset_y_position] }")

    def joy_callback(self, data):
        """仅用于存储最新的手柄数据"""
        self.latest_joy_data = data

    def control_loop_callback(self):
        """主控制循环，所有逻辑基于状态机在此分发"""
        if self.latest_joy_data is None:
            return
        
        if self.previous_buttons is None:
            self.previous_buttons = list(self.latest_joy_data.buttons)

        # 按钮事件检测 (检测按钮按下的瞬间)
        a_pressed = (self.latest_joy_data.buttons[BUTTON_A_INDEX] and not self.previous_buttons[BUTTON_A_INDEX])
        b_pressed = (self.latest_joy_data.buttons[BUTTON_B_INDEX] and not self.previous_buttons[BUTTON_B_INDEX])
        x_pressed = (self.latest_joy_data.buttons[BUTTON_X_INDEX] and not self.previous_buttons[BUTTON_X_INDEX])
        y_pressed = (self.latest_joy_data.buttons[BUTTON_Y_INDEX] and not self.previous_buttons[BUTTON_Y_INDEX])

        # ===============================================================
        # 4. 核心模式切换逻辑 (长按 RB)
        # ===============================================================
        rb_state = self.latest_joy_data.buttons[BUTTON_RB_INDEX]
        prev_rb_state = self.previous_buttons[BUTTON_RB_INDEX]

        if rb_state == 1 and prev_rb_state == 0:
            self.rb_press_time = self.get_clock().now()
        
        if rb_state == 0 and prev_rb_state == 1:
            if self.rb_press_time and (self.get_clock().now() - self.rb_press_time) >= self.long_press_duration:
                # 这是一个有效的长按，执行模式切换
                if self.control_mode == MODE_DOG_CONTROL:
                    self.get_logger().info("模式切换: [机器狗] -> [机械臂]")
                    self.control_mode = MODE_ARM_CONTROL
                    # 安全措施: 停止机器狗运动
                    self.send_dog_stop_command()
                    self.get_logger().info("--> 机械臂模式已激活。按 'A' 键启用控制, 'X' 键归位, 'Y' 键预设, 长按 'LB' 切换关节/笛卡尔模式。")
                elif self.control_mode == MODE_ARM_CONTROL:
                    self.get_logger().info("模式切换: [机械臂] -> [机器狗]")
                    self.control_mode = MODE_DOG_CONTROL
                    # 安全措施: 禁用机械臂伺服
                    if self.arm_servo.is_enabled:
                        self.arm_servo.disable()
                    self.is_arm_control_enabled = False
                    self.get_logger().info("--> 机器狗模式已激活。使用摇杆控制运动。")
                # 允许从“停止”模式直接切换到“机械臂”
                elif self.control_mode == MODE_STOP:
                    self.get_logger().info("模式切换: [停止] -> [机械臂]")
                    self.control_mode = MODE_ARM_CONTROL
                    self.get_logger().info("--> 机械臂模式已激活。按 'A' 键启用控制, 'X' 键归位, 'Y' 键预设, 长按 'LB' 切换关节/笛卡尔模式。")
            self.rb_press_time = None
        
        # ===============================================================
        # 5. 根据当前模式分发控制指令
        # ===============================================================
        # 状态: 停止 (MODE_STOP)
        if self.control_mode == MODE_STOP:
            if a_pressed:
                # 1. 首先检查机器狗物理系统是否就绪
                if not self.is_dog_physically_ready:
                    self.get_logger().warn("机器狗物理状态尚未就绪 (/robot_ready)，请等待...")
                
                # 2. 检查是否已站立
                elif self.dog_ctrl_state and self.dog_ctrl_state.standing:
                    self.get_logger().info("机器狗已处于站立状态, 直接切换到[机器狗控制]模式。")
                    self.control_mode = MODE_DOG_CONTROL
                
                # 3. 否则 (包括 dog_ctrl_state 为 None 或 standing 为 False)
                #    只要物理上就绪了，就应该执行站立动作
                else:
                    if self.dog_ctrl_state is None:
                        self.get_logger().info("检测到 'A' 键, 'standing' 状态未知 (假定未站立), 执行“准备”动作...")
                    else:
                        self.get_logger().info("检测到 'A' 键, 机器狗未站立, 执行“准备”动作序列...")
                    
                    self.send_dog_action_goal(DOG_ESTOP_ACTION_ID)
                    oneshot_timer = self.create_timer(0.5, lambda: self.stand_up_and_switch_to_dog_mode(oneshot_timer))

        # 状态: 机器狗控制 (MODE_DOG_CONTROL)
        elif self.control_mode == MODE_DOG_CONTROL:
            if y_pressed:
                self.get_logger().info("检测到 'Y' 键, 执行“趴下”动作, 切换到[停止]模式。")
                self.send_dog_action_goal(DOG_GET_DOWN_ACTION_ID)
                self.control_mode = MODE_STOP
            else:
                self.process_dog_movement()

        # 状态: 机械臂控制 (MODE_ARM_CONTROL)
        elif self.control_mode == MODE_ARM_CONTROL:
            self.process_arm_control(a_pressed, b_pressed, x_pressed, y_pressed)

        self.previous_buttons = list(self.latest_joy_data.buttons)

    # ===============================================================
    # 6. 机器狗功能函数
    # ===============================================================
    def dog_ready_callback(self, data):
        if data.ready != self.is_dog_physically_ready:
            self.is_dog_physically_ready = data.ready
            self.get_logger().info(f"接收到机器狗物理状态更新: is_ready = {self.is_dog_physically_ready}")
            if not self.is_dog_physically_ready and self.control_mode != MODE_ARM_CONTROL:
                self.control_mode = MODE_STOP
                self.get_logger().info("...因物理状态变为未就绪，逻辑状态强制重置为[停止]")

    def dog_state_callback(self, msg: DogCtrlStateStamped):
        """存储从 /alphadog_node/dog_ctrl_state 接收到的最新状态"""
        self.dog_ctrl_state = msg.state
        # 第一次接收到状态时打印日志
        if self.previous_buttons is None:
            self.get_logger().info("已成功接收到机器狗状态信息。")

    def send_dog_action_goal(self, action_id):
        if not self.dog_action_client.wait_for_server(timeout_sec=1.0):
            self.get_logger().error('机器狗 Action server 不可用!')
            return
        goal_msg = Execute.Goal()
        goal_msg.invoker = 'unified_joystick_controller'
        goal_msg.args = f'{{"action_id": {action_id}}}'
        self.get_logger().info(f'发送机器狗功能键指令, Action ID: {action_id}')
        self.dog_action_client.send_goal_async(goal_msg)
    
    def stand_up_and_switch_to_dog_mode(self, timer):
        """定时器回调，用于站立并切换到机器狗控制模式"""
        self.send_dog_action_goal(DOG_STAND_UP_ACTION_ID)
        self.control_mode = MODE_DOG_CONTROL
        self.get_logger().info("“准备”动作序列完毕。脚本现在处于[机器狗控制]模式。")
        self.get_logger().info("    使用左摇杆平移, 右摇杆旋转/俯仰。长按 'RB' 切换到机械臂。")
        timer.cancel()
        
    def process_dog_movement(self):
        """处理并发送机器狗的运动指令"""
        vel_msg = SetVelocity()
        rpy_msg = SetRpy()
        joy = self.latest_joy_data
        left_y = joy.axes[AXIS_LEFT_STICK_Y]
        left_x = joy.axes[AXIS_LEFT_STICK_X]
        right_y = joy.axes[AXIS_RIGHT_STICK_Y]
        right_x = joy.axes[AXIS_RIGHT_STICK_X]
        if left_y > 0: vel_msg.vx = left_y * DOG_FORWARD_SPEED_SCALE
        else: vel_msg.vx = left_y * DOG_BACKWARD_SPEED_SCALE
        vel_msg.vy = float(left_x * DOG_STRAFE_SPEED_SCALE)
        rpy_msg.pitch = float(right_y * DOG_PITCH_SCALE)
        rpy_msg.roll = 0.0
        is_moving = abs(left_y) > 0.1 or abs(left_x) > 0.1
        if is_moving:
            vel_msg.wz = float(right_x * DOG_ANGULAR_SPEED_SCALE)
            rpy_msg.yaw = 0.0
        else:
            vel_msg.wz = 0.0
            rpy_msg.yaw = float(right_x * DOG_YAW_ANGLE_SCALE)
        self.dog_velocity_pub.publish(vel_msg)
        self.dog_rpy_pub.publish(rpy_msg)
        
    def send_dog_stop_command(self):
        """发送零速指令以停止机器狗"""
        self.dog_velocity_pub.publish(SetVelocity())
        self.dog_rpy_pub.publish(SetRpy())

    # ===============================================================
    # 7. 机械臂功能函数
    # ===============================================================
    def go_to_arm_home_position(self):
        """
        将手臂和夹爪都归位到 0 (即 motor_bridge 中的 reset) 位置。
        """
        arm_cmd_msg = Float64MultiArray()
        arm_cmd_msg.data = list(self.arm_home_position) # [0.0, 0.0, 0.0, 0.0, 0.0]
        self.arm_direct_pos_pub.publish(arm_cmd_msg)
        
        self.last_commanded_positions = list(self.arm_home_position)

    def go_to_arm_preset_y_position(self):
        """
        发送 "Y 键" 预设位置指令。
        """
        arm_cmd_msg = Float64MultiArray()
        arm_cmd_msg.data = list(self.arm_preset_y_position)
        self.arm_direct_pos_pub.publish(arm_cmd_msg)
        
        self.last_commanded_positions = list(self.arm_preset_y_position)

    def joint_state_callback(self, msg: JointState):
        """统一回调，更新手臂和夹爪的当前位置"""
        
        # 1. 初始化手臂关节索引 (只执行一次)
        if not self.arm_joint_indices:
            try:
                self.arm_joint_indices = [msg.name.index(name) for name in ARM_JOINT_NAMES]
            except ValueError as e:
                self.get_logger().error(f"机械臂关节未在 /joint_states 中找到: {e}")
                self.arm_joint_indices = [-1] 
                return # 如果找不到索引，就不要继续了
        
        # 如果索引无效，也退出
        if self.arm_joint_indices[0] == -1:
            return

        # 2. 实时更新 current_arm_positions
        try:
            # 这一行必须在 `if not self.first_joint_state_received` 之外
            # 它现在会在每条消息中都更新手臂的当前 (偏移后) 位置
            self.current_arm_positions = [msg.position[i] for i in self.arm_joint_indices]
        except IndexError:
            self.get_logger().warn("IndexError processing joint states. Waiting for next message.", throttle_duration_sec=5.0)
            return

        # 3. 初始化 last_commanded_positions (仅一次)
        if not self.first_joint_state_received:
            # 第一次运行时, 使用刚更新的 current_arm_positions 来设置 last_commanded
            self.last_commanded_positions = list(self.current_arm_positions)
            self.first_joint_state_received = True
            self.get_logger().info(f"机械臂关节状态已接收。")
            self.get_logger().info(f"Initialized last_commanded_positions (offset-mapped): { [round(p, 2) for p in self.last_commanded_positions] }")

        # 4. 更新夹爪位置
        if self.gripper_joint_idx == -1:
            try:
                self.gripper_joint_idx = msg.name.index(GRIPPER_JOINT_NAME)
            except ValueError:
                self.get_logger().error(f"夹爪关节 '{GRIPPER_JOINT_NAME}' 未在 /joint_states 中找到。")
                self.gripper_joint_idx = -2 # 设为-2表示已尝试过，防止重复记录错误
        
        if self.gripper_joint_idx >= 0:
            try:
                self.current_gripper_pos = msg.position[self.gripper_joint_idx]
            except IndexError:
                self.get_logger().warn("IndexError processing gripper state.", throttle_duration_sec=5.0)
            
    def process_arm_control(self, a_pressed, b_pressed, x_pressed, y_pressed):
        """处理所有与机械臂相关的控制逻辑"""
        joy = self.latest_joy_data

        # 7.0 一键归位 (X 键)
        # 只要在机械臂模式下, 无论是否 "启用(A)" 都应生效
        if x_pressed:
            self.get_logger().info("按下 'X', [一键归位] 机械臂和夹爪到 0 位置。")
            self.go_to_arm_home_position()
            
            # 安全措施：如果正在笛卡尔模式, 禁用它
            if self.arm_servo.is_enabled:
                self.get_logger().info("...归位时禁用笛卡尔 Servo。")
                self.arm_servo.disable()
            
            # 安全措施：如果不在关节模式, 切换回关节模式
            if not self.is_arm_joint_mode:
                self.get_logger().info("...切换回 [关节模式] 以进行归位。")
                self.is_arm_joint_mode = True
                self.print_arm_mode_status()
            
            # 安全措施：归位后, 禁用手柄摇杆控制, 直到用户按下 A 键
            if self.is_arm_control_enabled:
                self.get_logger().info("...归位完成, 暂时 [禁用] 摇杆, 按 'A' 重新启用。")
                self.is_arm_control_enabled = False

            return
        
        # 7.0b Y 键预设位置
        if y_pressed:
            self.get_logger().info("按下 'Y', [一键移动] 机械臂和夹爪到 'Y' 预设位置。")
            self.go_to_arm_preset_y_position()
            
            # (复制 X 键的安全措施)
            if self.arm_servo.is_enabled:
                self.get_logger().info("...移动时禁用笛卡尔 Servo。")
                self.arm_servo.disable()
            if not self.is_arm_joint_mode:
                self.get_logger().info("...切换回 [关节模式] 以进行移动。")
                self.is_arm_joint_mode = True
                self.print_arm_mode_status()
            if self.is_arm_control_enabled:
                self.get_logger().info("...移动完成, 暂时 [禁用] 摇杆, 按 'A' 重新启用。")
                self.is_arm_control_enabled = False

        # 7.1 机械臂总使能开关 (A/B键)
        if a_pressed and not self.is_arm_control_enabled:
            self.get_logger().info("按下 'A', [启用] 机械臂控制。")
            self.is_arm_control_enabled = True
            if not self.is_arm_joint_mode:
                self.get_logger().info("...当前为笛卡尔模式, 启动Servo。")
                self.arm_servo.enable()
        
        if b_pressed and self.is_arm_control_enabled:
            self.get_logger().info("按下 'B', [禁用] 机械臂控制。")
            self.is_arm_control_enabled = False
            if self.arm_servo.is_enabled:
                self.arm_servo.disable()

        # 7.2 机械臂内部模式切换 (长按 LB)
        lb_state = joy.buttons[BUTTON_LB_INDEX]
        prev_lb_state = self.previous_buttons[BUTTON_LB_INDEX]

        if lb_state == 1 and prev_lb_state == 0:
            self.lb_press_time = self.get_clock().now()
        if lb_state == 0 and prev_lb_state == 1: # LB 键释放
            if self.lb_press_time and (self.get_clock().now() - self.lb_press_time) >= self.long_press_duration: # 确认是长按
                # 记录切换前的模式
                was_cartesian_mode = not self.is_arm_joint_mode

                # 切换模式
                self.is_arm_joint_mode = not self.is_arm_joint_mode
                self.print_arm_mode_status() # 打印新模式信息

                if self.is_arm_control_enabled:
                    if self.is_arm_joint_mode:
                        # 从笛卡尔切换到关节模式
                        if was_cartesian_mode:
                            self.get_logger().info("Disabling Servo and syncing last commanded positions.")
                            self.arm_servo.disable()
                            # 关键：用当前的实际关节位置更新 last_commanded_positions
                            # joint_state_callback 保证了 self.current_arm_positions 是最新的
                            if self.first_joint_state_received:
                                self.last_commanded_positions = list(self.current_arm_positions)
                                self.get_logger().info(f"Synced last_commanded_positions to current: { [round(p, 2) for p in self.last_commanded_positions] }")
                            else:
                                self.get_logger().warn("Cannot sync last_commanded_positions, joint states not yet received.")
                    else:
                        # 从关节切换到笛卡尔模式
                        self.get_logger().info("Enabling Servo for Cartesian control.")
                        self.arm_servo.enable()
            self.lb_press_time = None
        
        # 7.3 发送控制指令
        if self.is_arm_control_enabled:
            # 手臂运动
            if self.is_arm_joint_mode:
                self.send_arm_direct_joint_command(joy)
            else: # 笛卡尔模式
                if self.arm_servo.is_enabled:
                    self.send_arm_cartesian_command(joy)
            # 夹爪运动 (独立)
            self.handle_gripper_command(joy)
    
    def print_arm_mode_status(self):
        """打印关于机械臂当前控制模式的详细信息。"""
        gripper_mapping_info = f"      - 夹爪 张开/闭合: {self.axis_names.get(self.arm_cartesian_axis_map['gripper_open'])} / {self.axis_names.get(self.arm_cartesian_axis_map['gripper_close'])}\n"
        if self.is_arm_joint_mode:
            log_message = "\n切换到机械臂 [关节(Joint)] 控制模式\n"
            log_message += "    操作: 摇杆现在直接控制各个关节的速度。\n"
            log_message += "    映射:\n"
            log_message += gripper_mapping_info
            for i, joint_name in enumerate(ARM_JOINT_NAMES):
                axis_index = ARM_JOINT_AXIS_MAP[i]
                axis_desc = self.axis_names.get(axis_index, f"轴 {axis_index}")
                log_message += f"      - 关节 {joint_name}: {axis_desc}\n"
            self.get_logger().info(log_message)
        else: # Cartesian mode
            log_message = "\n切换到机械臂 [笛卡尔(Cartesian)] 控制模式\n"
            log_message += "    操作: 摇杆现在控制末端执行器的速度。\n"
            log_message += "    映射:\n"
            log_message += gripper_mapping_info
            log_message += f"      - X轴平移 (前后): {self.axis_names.get(self.arm_cartesian_axis_map['linear_y'])}\n"
            log_message += f"      - Y轴平移 (左右): {self.axis_names.get(self.arm_cartesian_axis_map['linear_x'])}\n"
            log_message += f"      - Z轴平移 (上下): {self.axis_names.get(self.arm_cartesian_axis_map['linear_z'])}\n"
            log_message += f"      - 俯仰 (Pitch):   {self.axis_names.get(self.arm_cartesian_axis_map['angular_y'])}\n"
            log_message += f"      - 偏航 (Yaw):     {self.axis_names.get(self.arm_cartesian_axis_map['angular_z'])}"
            self.get_logger().info(log_message)

    def send_arm_cartesian_command(self, msg: Joy):
        """发送笛卡尔空间速度指令 (已修正符号)"""
        linear = (
            msg.axes[self.arm_cartesian_axis_map["linear_y"]],
            msg.axes[self.arm_cartesian_axis_map["linear_x"]], # Y轴平移(左右)通常需要反号
            -msg.axes[self.arm_cartesian_axis_map["linear_z"]]  # Z轴平移(上下)也经常需要反号
        )
        angular = (
            0.0,
            msg.axes[self.arm_cartesian_axis_map["angular_y"]],
            msg.axes[self.arm_cartesian_axis_map["angular_z"]]
        )
        self.arm_servo(linear=linear, angular=angular)

    # ===============================================================
    # 8. 机械臂限制加载函数
    # ===============================================================
    def _load_joint_limits(self):
        """
        从校准文件加载关节限制, 并计算相对于复位位置的弧度限制。
        这复制了 motor_bridge.py 中的部分逻辑, 以确保限制一致。
        """
        self.get_logger().info("正在加载机械臂关节限制...")
        try:
            with open(CALIB_PATH, 'r') as f:
                loaded_data = json.load(f) # dict[str, dict]
            
            _steps_per_rad = 4096.0 / (2 * math.pi)

            for name in ARM_JOINT_NAMES:
                if name not in loaded_data or name not in JOINTS or name not in RESET_NORMALIZED_GOALS:
                    self.get_logger().warn(f"跳过关节 {name}: 在校准文件或常量中未找到。")
                    continue
                
                cal = loaded_data[name] # dict
                joint_cfg = JOINTS[name]
                norm_mode_str = joint_cfg["mode"]
                
                # 从校准文件获取原始限制
                raw_min = cal["range_min"]
                raw_max = cal["range_max"]
                home = cal["homing_offset"]

                # 1. 计算复位位置的【真实弧度】(同 motor_bridge)
                norm_val = RESET_NORMALIZED_GOALS[name]
                reset_raw = 0
                if (raw_max - raw_min) == 0: continue # 避免除零

                if norm_mode_str == MotorNormMode.RANGE_M100_100:
                    reset_raw = int(((norm_val + 100) / 200) * (raw_max - raw_min) + raw_min)
                elif norm_mode_str == MotorNormMode.RANGE_0_100:
                    reset_raw = int((norm_val / 100) * (raw_max - raw_min) + raw_min)
                
                reset_rad_offset = (reset_raw - home) * (2 * math.pi) / 4096.0

                # 2. 计算物理限制的【真实弧度】
                min_true_rad = (raw_min - home) * (2 * math.pi) / 4096.0
                max_true_rad = (raw_max - home) * (2 * math.pi) / 4096.0

                # 3. 计算相对于复位位置的【相对弧度限制】
                # 这就是 arm_controller 命令 (如 0.0) 所对应的限制
                min_relative_rad = min_true_rad - reset_rad_offset
                max_relative_rad = max_true_rad - reset_rad_offset
                
                # 保证 min < max
                self.arm_relative_rad_limits[name] = (min(min_relative_rad, max_relative_rad), 
                                                      max(min_relative_rad, max_relative_rad))

            self.get_logger().info("成功加载并计算了机械臂相对弧度限制:")
            for name, (min_r, max_r) in self.arm_relative_rad_limits.items():
                 self.get_logger().info(f"  - 关节 {name}: ({min_r:.2f}, {max_r:.2f}) rad")


        except FileNotFoundError:
            self.get_logger().error(f"校准文件未找到: {CALIB_PATH}")
            self.get_logger().error("无法加载关节限制！关节模式将没有限制！")
        except Exception as e:
            self.get_logger().error(f"加载/计算关节限制时出错: {e}")
            import traceback
            self.get_logger().error(traceback.format_exc())

    # ===============================================================
    # 9. 机械臂关节控制函数
    # ===============================================================
    def send_arm_direct_joint_command(self, msg: Joy):
        """直接发送关节空间位置指令 - 使用最后命令位置作为基准并应用限制"""
        if not self.arm_joint_indices or self.arm_joint_indices[0] == -1 or not self.first_joint_state_received:
            # 如果关节索引无效或尚未初始化 last_commanded_positions，则不执行
            return

        dt = 0.02 # 假设循环频率为50Hz
        # 使用上一次命令的目标位置作为本次目标的基础
        target_positions = list(self.last_commanded_positions)
        active_joint = False

        for i, axis_index in enumerate(ARM_JOINT_AXIS_MAP):
            axis_val = -msg.axes[axis_index] # 保持手柄符号逻辑

            if abs(axis_val) > 0.1: # 死区
                active_joint = True
                
                joint_name = ARM_JOINT_NAMES[i]
                
                # 1. 计算增量
                delta = axis_val * ARM_JOINT_VELOCITY_GAIN * dt
                
                # 2. 计算不带限制的潜在新位置
                potential_new_pos = self.last_commanded_positions[i] + delta
                
                # 3. 应用限制 (钳位)
                clamped_pos = potential_new_pos
                if joint_name in self.arm_relative_rad_limits:
                    min_rad, max_rad = self.arm_relative_rad_limits[joint_name]
                    clamped_pos = max(min_rad, min(max_rad, potential_new_pos))
                    
                    # (可选的日志) 如果被钳位，给一个温和的日志提示
                    if clamped_pos != potential_new_pos:
                        self.get_logger().warn(f"关节 {joint_name} 达到限制。", throttle_duration_sec=2.0)

                # 4. 将（钳位后的）位置设置为本轮目标
                target_positions[i] = clamped_pos


        # 只要手柄有输入就发送命令
        if active_joint: 
            cmd_msg = Float64MultiArray()
            cmd_msg.data = target_positions
            self.arm_direct_pos_pub.publish(cmd_msg)
            
            # 用本轮计算好(并钳位后)的目标位置, 更新“上一轮命令位置”
            # 这样, "积分器" self.last_commanded_positions 自身永远不会累积到超过限制
            self.last_commanded_positions = list(target_positions)
            
        # 如果没有摇杆输入 (active_joint=False), 则:
        # 1. 不发布新命令 (arm_controller 会保持在 last_commanded_positions)
        # 2. self.last_commanded_positions 保持不变
        
    def handle_gripper_command(self, msg: Joy):
        """处理夹爪的开合指令 (点动逻辑)"""
        if self.gripper_joint_idx < 0: return

        rt_val = msg.axes[AXIS_RT] # [-1.0, 1.0], 初始为1.0
        lt_val = msg.axes[AXIS_LT] # [-1.0, 1.0], 初始为1.0
        dt = 0.02 
        
        # 每次都从当前实际位置开始计算, 这可以防止积分累计 (实现点动)
        target_pos = self.current_gripper_pos 
        
        is_active = False
        
        # LT (闭合)
        if lt_val < 1.0: 
            is_active = True
            normalized_press = (lt_val - 1.0) / -2.0  # 映射到 [0, 1]
            delta = GRIPPER_JOG_SPEED * normalized_press * dt
            target_pos -= delta
        # RT (张开)
        elif rt_val < 1.0:
            is_active = True
            normalized_press = (rt_val - 1.0) / -2.0 # 映射到 [0, 1]
            delta = GRIPPER_JOG_SPEED * normalized_press * dt
            target_pos += delta

        # 只有在扳机被按下时才发送命令
        if is_active:
            clamped_pos = max(GRIPPER_CLOSED_POS, min(GRIPPER_OPEN_POS, target_pos))
            cmd_msg = Float64MultiArray()
            cmd_msg.data = [clamped_pos]
            self.gripper_pub.publish(cmd_msg)

def main(args=None):
    rclpy.init(args=args)
    unified_node = UnifiedJoystickController()
    rclpy.spin(unified_node)
    unified_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()